#include "ImuSensor.h"
#include "delay.h"

static SpiPeripheral spi2(SPI2);
#define STATIC_DRIFT_CNT 100

/**
* @Axis Z(Yaw), Y(Roll), X(Pitch)
*       Z
*       ^
*       |
*       |    Y
*       |   /
*       |  /
*       | /
*       |/
*       O-------------->X
*/

ImuSensor::ImuSensor(SpiPeripheral* trans)
{
    _ptrans = trans;
    X_Last_ODR = 104.0f;
    X_isEnabled = 0;

    G_Last_ODR = 104.0f;
    G_isEnabled = 0;
    g_sensitivity = 0;
    x_sensitivity = 0;
    angular_zero_rate = 0;
}

ImuSensor* ImuSensor::get(void)
{
    static ImuSensor imu_(&spi2);
    return &imu_;
}

u8 ImuSensor::readRegister(u8 regaddr)
{
    u8 ret = 0;
    _ptrans->chipSelect();
    _ptrans->readAfterWrite(regaddr|0x80);
    ret = _ptrans->readAfterWrite(0xFF);
    _ptrans->chipRelease();
    return ret;
}

u8 ImuSensor::getDeviceId(void)
{
    u8 ret = 0;
    ret = readRegister(LSM6DSL_WHO_AM_I);
    return ret;
}

bool ImuSensor::init(void)
{
    u8 ret;
    _ptrans->begin();
    /**
    * LSM6DSL_CTRL3_C == 0x04 
    * means IF_INC = 1, ie Register address automatically incremented during a multiple byte access
    */
    this->IO_Read(&ret, LSM6DSL_CTRL3_C, 1);
    if(getDeviceId() == device_id)
        return true;
    else
        return false;
}

/**
 * @brief  Enable LSM6DSL Accelerator
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Enable_X()
{
    /* Check if the component is already enabled */
    if ( X_isEnabled == 1 )
    {
        return LSM6DSL_STATUS_OK;
    }

    /* Output data rate selection. */
    if ( Set_X_ODR_When_Enabled( X_Last_ODR ) == LSM6DSL_STATUS_ERROR )
    {
        return LSM6DSL_STATUS_ERROR;
    }
    
    /* Get LSM6DSL actual sensitivity. */
    if ( Get_X_Sensitivity( &x_sensitivity ) == LSM6DSL_STATUS_ERROR )
    {
    return LSM6DSL_STATUS_ERROR;
    }

    X_isEnabled = 1;

    return LSM6DSL_STATUS_OK;
}

void _LSM6DSL_ACC_GYRO_W_ODR_XL(void* handler,  LSM6DSL_ACC_GYRO_ODR_XL_t newValue)
{
    u8 val;
    ImuSensor* pImu = (ImuSensor*)handler;
    pImu->IO_Read(&val, LSM6DSL_CTRL1_XL, 1);
    
    val &= ~LSM6DSL_ACC_GYRO_ODR_XL_MASK;
    val |= newValue;   
    pImu->IO_Write(&val, LSM6DSL_CTRL1_XL, 1);
}

/**
 * @brief  Set LSM6DSL Accelerometer output data rate when enabled
 * @param  odr the output data rate to be set
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Set_X_ODR_When_Enabled(float odr)
{
  LSM6DSL_ACC_GYRO_ODR_XL_t new_odr;

  new_odr = ( odr <=   13.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_13Hz
          : ( odr <=   26.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_26Hz
          : ( odr <=   52.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_52Hz
          : ( odr <=  104.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_104Hz
          : ( odr <=  208.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_208Hz
          : ( odr <=  416.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_416Hz
          : ( odr <=  833.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_833Hz
          : ( odr <= 1660.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_1660Hz
          : ( odr <= 3330.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_3330Hz
          :                      LSM6DSL_ACC_GYRO_ODR_XL_6660Hz;

  _LSM6DSL_ACC_GYRO_W_ODR_XL((void*)this, new_odr);

  return LSM6DSL_STATUS_OK;
}


/**
* @brief 初始化静态偏移，阻塞
*/
void ImuSensor::init_static_drift(void)
{
  const int cnt = STATIC_DRIFT_CNT;
  double ave = 0.0;
  int32_t static_angle[3] = {0};
  this->angular_zero_rate = 0;
  
  for(int i=0;i<cnt;i++)
  {
      this->Get_G_Axes(static_angle);
      delay_ms(1);
    //  delay_ms(1);
      ave += static_angle[2];
  }
  
  ave = ave*1.0f/cnt;
  this->angular_zero_rate = ave;
}

/**
* @brief 更新静态角速度，读一个数据，刷新静态偏移值
*/
void ImuSensor::refresh_zero_rate(void)
{
  int32_t static_angle[3] = {0};
  this->Get_G_Axes(static_angle);
  this->angular_zero_rate = static_angle[2]*1.0/STATIC_DRIFT_CNT + this->angular_zero_rate*(STATIC_DRIFT_CNT-1)/STATIC_DRIFT_CNT;
}

/**
 * @brief  Enable LSM6DSL Gyroscope
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Enable_G(void)
{
//  int32_t static_angle[3] = {0};    //静止时的角速度
//  const int init_cnt = 100;

  /* Check if the component is already enabled */
  if ( G_isEnabled == 1 )
  {
    return LSM6DSL_STATUS_OK;
  }

  /* Output data rate selection. */
  if ( Set_G_ODR_When_Enabled( G_Last_ODR ) == LSM6DSL_STATUS_ERROR )
  {
    return LSM6DSL_STATUS_ERROR;
  }
  
  /* Get LSM6DSL actual sensitivity. */
  if ( Get_G_Sensitivity( &g_sensitivity ) == LSM6DSL_STATUS_ERROR )
  {
    return LSM6DSL_STATUS_ERROR;
  }

  G_isEnabled = 1;
  
//  for(int i=0;i<init_cnt;i++)
//  {
//      this->Get_G_Axes(static_angle);
//      this->angular_zero_rate += static_angle[2];
//  }
//  
//  this->angular_zero_rate = angular_zero_rate/init_cnt*1.3;

  return LSM6DSL_STATUS_OK;
}

void _LSM6DSL_ACC_GYRO_W_ODR_G(void* handler,  LSM6DSL_ACC_GYRO_ODR_G_t newValue)
{
    u8 val;
    ImuSensor* pImu = (ImuSensor*)handler;
    pImu->IO_Read(&val, LSM6DSL_CTRL2_G, 1);
    
    val &= ~LSM6DSL_ACC_GYRO_ODR_G_MASK;
    val |= newValue;
    
    pImu->IO_Write(&val, LSM6DSL_CTRL2_G, 1);
}

/**
 * @brief  Set LSM6DSL Gyroscope output data rate when enabled
 * @param  odr the output data rate to be set
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Set_G_ODR_When_Enabled(float odr)
{
  LSM6DSL_ACC_GYRO_ODR_G_t new_odr;

  new_odr = ( odr <=  13.0f )  ? LSM6DSL_ACC_GYRO_ODR_G_13Hz
          : ( odr <=  26.0f )  ? LSM6DSL_ACC_GYRO_ODR_G_26Hz
          : ( odr <=  52.0f )  ? LSM6DSL_ACC_GYRO_ODR_G_52Hz
          : ( odr <= 104.0f )  ? LSM6DSL_ACC_GYRO_ODR_G_104Hz
          : ( odr <= 208.0f )  ? LSM6DSL_ACC_GYRO_ODR_G_208Hz
          : ( odr <= 416.0f )  ? LSM6DSL_ACC_GYRO_ODR_G_416Hz
          : ( odr <= 833.0f )  ? LSM6DSL_ACC_GYRO_ODR_G_833Hz
          : ( odr <= 1660.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_1660Hz
          : ( odr <= 3330.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_3330Hz
          :                      LSM6DSL_ACC_GYRO_ODR_G_6660Hz;

  _LSM6DSL_ACC_GYRO_W_ODR_G( (void *)this, new_odr );

  return LSM6DSL_STATUS_OK;
}

/**
 * @brief  Read data from LSM6DSL Gyroscope
 * @param  pData the pointer where the gyroscope data are stored
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Get_G_Axes(int32_t *pData)
{
  int16_t dataRaw[3];

  /* Read raw data from LSM6DSL output register. */
  if ( Get_G_AxesRaw( dataRaw ) == LSM6DSL_STATUS_ERROR )
  {
    return LSM6DSL_STATUS_ERROR;
  }

  /* Calculate the data. */
  pData[0] = ( int32_t )( dataRaw[0] * g_sensitivity );
  pData[1] = ( int32_t )( dataRaw[1] * g_sensitivity );
  pData[2] = ( int32_t )( dataRaw[2] * g_sensitivity );

  return LSM6DSL_STATUS_OK;
}

void _LSM6DSL_ACC_GYRO_GetRawGyroData(void *handler, u8 *buff)
{
  u8 i, j, k;
  u8 numberOfByteForDimension;
  ImuSensor* pImu = (ImuSensor*)handler;

  numberOfByteForDimension=6/3;

  k=0;
  for (i=0; i<3;i++ )
  {
    for (j=0; j<numberOfByteForDimension;j++ )
    {
        pImu->IO_Read(&buff[k], LSM6DSL_OUTX_L_G+k, 1);
        k++;
    }
  }
}

/**
 * @brief  Read raw data from LSM6DSL Gyroscope
 * @param  pData the pointer where the gyroscope raw data are stored
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Get_G_AxesRaw(int16_t *pData)
{
  uint8_t regValue[6] = {0, 0, 0, 0, 0, 0};

  /* Read output registers from LSM6DSL_ACC_GYRO_OUTX_L_G to LSM6DSL_ACC_GYRO_OUTZ_H_G. */
  _LSM6DSL_ACC_GYRO_GetRawGyroData( (void *)this, regValue );

  /* Format the data. */
  pData[0] = ( ( ( ( int16_t )regValue[1] ) << 8 ) + ( int16_t )regValue[0] );
  pData[1] = ( ( ( ( int16_t )regValue[3] ) << 8 ) + ( int16_t )regValue[2] );
  pData[2] = ( ( ( ( int16_t )regValue[5] ) << 8 ) + ( int16_t )regValue[4] );

  return LSM6DSL_STATUS_OK;
}

/**
* @brief 判断是否选用125dps
* ODRG[bit7:4]:Output Data Rate(ODR)
* FS_G[bit3:2]:full scale (245, 500, 1000, 2000)
* FS_125[bit1]:full scale是否为125
* bit0: always zero
*/
void _LSM6DSL_ACC_GYRO_R_FS_125(void *handler, LSM6DSL_ACC_GYRO_FS_125_t *value)
{
    ImuSensor* pImu = (ImuSensor*)handler;
    pImu->IO_Read((u8 *)value, LSM6DSL_CTRL2_G, 1);

    *(u8*)value &= LSM6DSL_ACC_GYRO_FS_125_MASK; //mask
}

void _LSM6DSL_ACC_GYRO_R_FS_G(void *handler, LSM6DSL_ACC_GYRO_FS_G_t *value)
{
    ImuSensor* pImu = (ImuSensor*)handler;
    pImu->IO_Read((u8 *)value, LSM6DSL_CTRL2_G, 1);

    *(u8*)value &= LSM6DSL_ACC_GYRO_FS_G_MASK; //mask
}

/**
 * @brief  Read Gyroscope Sensitivity
 * @param  pfData the pointer where the gyroscope sensitivity is stored
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Get_G_Sensitivity(float *pfData)
{
  LSM6DSL_ACC_GYRO_FS_125_t fullScale125;
  LSM6DSL_ACC_GYRO_FS_G_t   fullScale;

  /* Read full scale 125 selection from sensor. */
  _LSM6DSL_ACC_GYRO_R_FS_125( (void *)this, &fullScale125 );

  if ( fullScale125 == LSM6DSL_ACC_GYRO_FS_125_ENABLED )
  {
    *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_125DPS;
  }

  else
  {

    /* Read actual full scale selection from sensor. */
    _LSM6DSL_ACC_GYRO_R_FS_G( (void *)this, &fullScale );

    /* Store the sensitivity based on actual full scale. */
    switch( fullScale )
    {
      case LSM6DSL_ACC_GYRO_FS_G_245dps:
        *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_245DPS;
        break;
      case LSM6DSL_ACC_GYRO_FS_G_500dps:
        *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_500DPS;
        break;
      case LSM6DSL_ACC_GYRO_FS_G_1000dps:
        *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_1000DPS;
        break;
      case LSM6DSL_ACC_GYRO_FS_G_2000dps:
        *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_2000DPS;
        break;
      default:
        *pfData = -1.0f;
        return LSM6DSL_STATUS_ERROR;
    }
  }

  return LSM6DSL_STATUS_OK;
}

/**
 * @brief  Read data from LSM6DSL Accelerometer
 * @param  pData the pointer where the accelerometer data are stored
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Get_X_Axes(int32_t *pData)
{
  int16_t dataRaw[3];

  /* Read raw data from LSM6DSL output register. */
  if ( Get_X_AxesRaw( dataRaw ) == LSM6DSL_STATUS_ERROR )
  {
    return LSM6DSL_STATUS_ERROR;
  }

  /* Calculate the data. */
  pData[0] = ( int32_t )( dataRaw[0] * x_sensitivity );
  pData[1] = ( int32_t )( dataRaw[1] * x_sensitivity );
  pData[2] = ( int32_t )( dataRaw[2] * x_sensitivity );

  return LSM6DSL_STATUS_OK;
}

void _LSM6DSL_ACC_GYRO_R_FS_XL(void *handler, LSM6DSL_ACC_GYRO_FS_XL_t *value)
{
    ImuSensor* pImu = (ImuSensor*)handler;
    pImu->IO_Read((u8 *)value, LSM6DSL_CTRL1_XL, 1);

    *(u8 *)value &= LSM6DSL_ACC_GYRO_FS_XL_MASK; //mask
}

/**
 * @brief  Read Accelerometer Sensitivity
 * @param  pfData the pointer where the accelerometer sensitivity is stored
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Get_X_Sensitivity(float *pfData)
{
  LSM6DSL_ACC_GYRO_FS_XL_t fullScale;

  /* Read actual full scale selection from sensor. */
  _LSM6DSL_ACC_GYRO_R_FS_XL( (void *)this, &fullScale );

  /* Store the sensitivity based on actual full scale. */
  switch( fullScale )
  {
    case LSM6DSL_ACC_GYRO_FS_XL_2g:
      *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_2G;
      break;
    case LSM6DSL_ACC_GYRO_FS_XL_4g:
      *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_4G;
      break;
    case LSM6DSL_ACC_GYRO_FS_XL_8g:
      *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_8G;
      break;
    case LSM6DSL_ACC_GYRO_FS_XL_16g:
      *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_16G;
      break;
    default:
      *pfData = -1.0f;
      return LSM6DSL_STATUS_ERROR;
  }

  return LSM6DSL_STATUS_OK;
}

void _LSM6DSL_ACC_GYRO_GetRawAccData(void *handler, u8 *buff)
{
  u8 i, j, k;
  u8 numberOfByteForDimension;
  ImuSensor* pImu = (ImuSensor*)handler;

  numberOfByteForDimension=6/3;

  k=0;
  for (i=0; i<3;i++ )
  {
    for (j=0; j<numberOfByteForDimension;j++ )
    {
        pImu->IO_Read(&buff[k], LSM6DSL_OUTX_L_XL+k, 1);
        k++;
    }
  }
}

/**
 * @brief  Read raw data from LSM6DSL Accelerometer
 * @param  pData the pointer where the accelerometer raw data are stored
 * @retval LSM6DSL_STATUS_OK in case of success, an error code otherwise
 */
LSM6DSLStatusTypeDef ImuSensor::Get_X_AxesRaw(int16_t *pData)
{
  uint8_t regValue[6] = {0, 0, 0, 0, 0, 0};

  /* Read output registers from LSM6DSL_ACC_GYRO_OUTX_L_XL to LSM6DSL_ACC_GYRO_OUTZ_H_XL. */
  _LSM6DSL_ACC_GYRO_GetRawAccData( (void *)this, regValue );

  /* Format the data. */
  pData[0] = ( ( ( ( int16_t )regValue[1] ) << 8 ) + ( int16_t )regValue[0] );
  pData[1] = ( ( ( ( int16_t )regValue[3] ) << 8 ) + ( int16_t )regValue[2] );
  pData[2] = ( ( ( ( int16_t )regValue[5] ) << 8 ) + ( int16_t )regValue[4] );

  return LSM6DSL_STATUS_OK;
}

/* EOF */
